#include <iostream>
#include "bodytraced_guidance.h"
#include "px4_interface.h"
#include "guidance_filter/BlosSim.h"
// #include "guidance_filter/PosFilter.h"

int main() {
    // 获取飞机状态
    Px4Interface px4_interface(26540, 24580, "127.0.0.1");
    UavStatus px4_info;

    // 获取模拟的视线角信息
//    ZmqClient  zmq_client("/Target/1", "192.168.8.253", 14552, 40);
    TargetInfo target_info;
    LineOfSight blos, blos_rate;

    // 制导控制器
    ControllerInterface*  controller_ptr;
    controller_ptr = new BodyTracedGuidance;
    mavlink_set_attitude_target_t att_cmd;

    uint64_t timer_us = currentTimeUs();
    target_info.index = 1;
    target_info.alt = 1;
    target_info.lat = 32.5691884;
    target_info.lon = 116.7273203;

//    PosKalmanFilter kalman_(0.1);
//    kalman_.setMeansUncertainty(0.01f);
//    float v_kalman, h_kalman, v_rate_kalman, h_rate_kalman;

//    uint8_t seq{0};

    while(true) {
        // 判断是否获得飞控信息
        if (px4_interface.updated(px4_info) && px4_info.home_pos.time_usec > 0) {

            // 从zmq协议获取目标位置
//            zmq_client.updated(target_info);

            // 若目标位置更新足够快
            uint64_t time_diff = elapsedTimeMs(target_info.time_stamp_ms);
            if (1) {

                // 根据相对位置关系，模拟视线角
                pos_to_BLOS(px4_info, target_info, blos);

                // 计算飞行姿态信息，roll， pitch， yaw
                matrix::Eulerf euler_rad = matrix::Eulerf (matrix::Quatf(px4_info.att_q.q1,px4_info.att_q.q2,px4_info.att_q.q3,px4_info.att_q.q4));
                controller_ptr->set_att_status(euler_rad(0), euler_rad(1), euler_rad(2));

                // 给控制器视线角信息输入
//                kalman_.update(blos.vertical, blos.horizontal,
//                               v_kalman, h_kalman, v_rate_kalman, h_rate_kalman);
//                if (seq < 5) {
//                    seq++;
//                } else
//                {
                    controller_ptr->set_LOS_status(blos.vertical, blos.horizontal,
                                                   blos_rate.vertical, blos_rate.horizontal);
                    controller_ptr->set_airspeed(px4_info.vfr_hud.groundspeed);

                    // 计算制导控制量
                    controller_ptr->calc_att_thr_cmd(att_cmd);

                    // 制导控制量发给飞控
                    px4_interface.send_att_cmd_to_px4(att_cmd);
//                }

//                std::cout << "main loop time " << (int)elapsedTimeUs(timer_us) << " us. " << std::endl;
                timer_us = currentTimeUs();

//              by Zeratul_Zhang
//              算到目标的实时横纵距离
                mavlink_local_position_ned_t px4_local_pos;
                float N_target, E_target, D_target;
                globallocalconverter_tolocal(target_info.lat, target_info.lon, target_info.alt,
                                             &N_target, &E_target, &D_target);
                globallocalconverter_tolocal(px4_info.globalPos.lat * 1e-7,
                                             px4_info.globalPos.lon * 1e-7,
                                             px4_info.globalPos.alt * 1e-3f,
                                             &px4_local_pos.x, &px4_local_pos.y, &px4_local_pos.z);
//                std::cout << N_target <<" "<< E_target << " " << D_target <<std::endl;
//                std::cout << px4_local_pos.x << " " << px4_local_pos.y << " " << px4_local_pos.z <<std::endl;
                float deta_x = abs(px4_local_pos.x - N_target);
                float deta_y = abs(px4_local_pos.y - E_target);
                float deta_z = abs(px4_local_pos.z) - abs(D_target)-1;
                float h_dis = sqrt(pow(deta_x,2) + pow(deta_y,2));
                std::cout << "飞机自身高度： " << -(px4_local_pos.z) << ' ';
                std::cout << "水平方向距离： "
                          << h_dis << " "
                          << "垂直方向距离： "
                          << deta_z << " "
                          << std::endl;
//              by Zeratul_Zhang

            } else {
                // 大于1s没有获取目标信息，就退出制导
                px4_interface.terminate_offboard();
            }

        } else {
            // 没有获取到飞控信息，等待
            usleep(50 * 1000);
        }

    }

    return 0;
}
